package mainPackage;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;

import Maps.DownStairsMap;
import Maps.UpStairsMap;
import Utils.Coordinates;
import Utils.LightSensorMethod;
import Utils.MovementMethod;
import Utils.TouchSensorMethod;
import Utils.UltrasonicSensorMethod;
import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.PilotProps;


public class Main {

	static RegulatedMotor leftMotor;
	static RegulatedMotor rightMotor;
	
	public static int foundTheKing = -1;
	public static Coordinates kingCoordinate = new Coordinates();
	
	public static boolean foundThePrincess = false;
	
	public static final int FOLLOW_LEFT_WALL = 0;
	public static final int FOLLOW_RIGHT_WALL = 1;
	public static final int EXPLORE_RIGHT = 2;
	public static final int EXPLORE_LEFT = 3;
	
	public static final int MOVE_TO_RAMP_FIRST_FLOOR = 4;
	public static final int CLIMB_RAMP = 5;
	public static final int EXPLORE_SECOND_FLOOR = 6;
	public static final int FIND_PRINCESS = 7;
	public static final int MOVE_TO_RAMP_SECOND_FLOOR = 8;
	
	public static final int CLIMB_RAMP_DOWN = 9;
	
	public static final int RETURN_PRINCESS_START_AREA = 10;
	public static final int RETURN_PRINCESS_TO_KING = 11;
	
	public static DownStairsMap myLevel1 = new DownStairsMap(); 
	public static UpStairsMap myLevel2 = new UpStairsMap(); 
	
	public static void main(String[] args) throws Exception {
		
		/////////////////////////////////////////////////////////////////////////////////////////
		///////////////////////////////////// SETTINGS //////////////////////////////////////////
		/////////////////////////////////////////////////////////////////////////////////////////
		
		PilotProps pp = new PilotProps();
		pp.loadPersistentValues();
		
		float wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER, "5.48"));
		float trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH, "15.45"));
		leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR, "A"));
		rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR, "C"));
		boolean reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE, "false"));

		DifferentialPilot myRobot = new DifferentialPilot(wheelDiameter,trackWidth, leftMotor, rightMotor, reverse);
		
		myRobot.setAcceleration(400);
		myRobot.setTravelSpeed(20); // cm/sec
		myRobot.setRotateSpeed(90); // deg/sec

		UltrasonicSensor myUltrasonicSensor = new UltrasonicSensor(SensorPort.S1);
		LightSensor myLightSensor = new LightSensor(SensorPort.S2);
		myLightSensor.setFloodlight(true);
		
		TouchSensor myRightTouchSensor = new TouchSensor(SensorPort.S3);
		TouchSensor myLeftTouchSensor = new TouchSensor(SensorPort.S4);
		
		
		BTConnection btc = Bluetooth.waitForConnection();
		
		InputStream is = btc.openInputStream();
		OutputStream os = btc.openOutputStream();

		DataOutputStream bOutStr = new DataOutputStream(os);
		DataInputStream bInStr = new DataInputStream(is);
		
		int angleInterval = 15;
		/////////////////////////////////////////////////////////////////////////////////////////
		/////////////////////// Localize in the Starting Zone  //////////////////////////////////
		/////////////////////////////////////////////////////////////////////////////////////////

		// Press a Button to start program
		Button.waitForPress();
		Coordinates myCoord = new Coordinates();

		// Move our robot to Left Down Corner [0,0]
		MovementMethod.localizeInStartingZone(myRobot, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor, angleInterval);
		
		travelToRamp (myCoord, bOutStr, bInStr, myRobot, myLightSensor, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor );
		
		climbUpTheRamp(bOutStr, bInStr, myRobot, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor);
		
		exploreSecondFloor(myCoord, bOutStr, bInStr, myRobot, myLightSensor, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor);
		
		if (foundThePrincess == true){
			
			climbDownTheRamp(bOutStr, bInStr, myRobot, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor);
			
			if(foundTheKing == -1) {
				moveToStartingZone(myCoord, bOutStr, bInStr, myRobot, myLightSensor, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor);
			} else {
				moveToTheKing(myCoord, bOutStr, bInStr, myRobot, myLightSensor, myUltrasonicSensor, myLeftTouchSensor, myRightTouchSensor);
			}
		}
	}
	
	public static void travelToRamp (Coordinates myCoord, DataOutputStream bOutStr, DataInputStream bInStr, DifferentialPilot mRobot, LightSensor mLightSensor, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor ) throws InterruptedException, IOException {
	
		
		int movementType = FOLLOW_LEFT_WALL;
		
		while (myCoord.yCoord != 10) {
			
			if( movementType == FOLLOW_LEFT_WALL ) {
				
				if( UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor) > 20) {
					
					MovementMethod.moveOneCellForward(mRobot);
					
					myCoord.yCoord++;
					
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
						foundTheKing = FOLLOW_LEFT_WALL;
						kingCoordinate.xCoord = myCoord.xCoord;
						kingCoordinate.yCoord = myCoord.yCoord;
					}
					
					sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], 1 ,myCoord.xCoord, myCoord.yCoord) ;
					
					movementType = EXPLORE_RIGHT;
					
					
				} else {
					
					int yNextCoordinate = myCoord.yCoord + 1;
					
					int direction = 1;
					
					while (myCoord.yCoord != yNextCoordinate ) {
						
						int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
						int leftWallDistance = UltrasonicSensorMethod.getLeftDistance(mUltrasonicSensor);
						
						if ( leftWallDistance > 30 ) {
							MovementMethod.turnLeft(mRobot);
							MovementMethod.moveOneCellForward(mRobot);
							direction = MovementMethod.changeDirectionTurnLeft(direction);
							MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
							myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
							if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
								foundTheKing = FOLLOW_LEFT_WALL;
								kingCoordinate.xCoord = myCoord.xCoord;
								kingCoordinate.yCoord = myCoord.yCoord;
							}
							sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;							
						} else if ( forwardWallDistance > 30 ) {
							MovementMethod.moveOneCellForward(mRobot);
							MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
							myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
							if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
								foundTheKing = FOLLOW_LEFT_WALL;
								kingCoordinate.xCoord = myCoord.xCoord;
								kingCoordinate.yCoord = myCoord.yCoord;
							}
							sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
						} else {
							TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
							MovementMethod.turnRight(mRobot);
							direction = MovementMethod.changeDirectionTurnRight(direction);
							sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
						}
					}
					
					movementType = EXPLORE_RIGHT;
				}
				
			} else if ( movementType == FOLLOW_RIGHT_WALL ) {
				
				if( UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor) > 20) {
					
					MovementMethod.moveOneCellForward(mRobot);
					
					myCoord.yCoord++;
					
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
						foundTheKing = FOLLOW_RIGHT_WALL;
						kingCoordinate.xCoord = myCoord.xCoord;
						kingCoordinate.yCoord = myCoord.yCoord;
					}
					sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], 1 ,myCoord.xCoord, myCoord.yCoord) ;
					
					movementType = EXPLORE_LEFT;
			
				} else {
					
					int yNextCoordinate = myCoord.yCoord + 1;
					
					int direction = 1;
					
					while ( myCoord.yCoord != yNextCoordinate ) {
						
						int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
						int rightWallDistance = UltrasonicSensorMethod.getRightDistance(mUltrasonicSensor);
						
						if ( rightWallDistance > 30 ) {
							MovementMethod.turnRight(mRobot);
							MovementMethod.moveOneCellForward(mRobot);
							direction = MovementMethod.changeDirectionTurnRight(direction);
							MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
							myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
							if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
								foundTheKing = FOLLOW_RIGHT_WALL;
								kingCoordinate.xCoord = myCoord.xCoord;
								kingCoordinate.yCoord = myCoord.yCoord;
							}
							sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
							
						} else if ( forwardWallDistance > 30 ) {
							MovementMethod.moveOneCellForward(mRobot);
							MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
							myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
							if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
								foundTheKing = FOLLOW_RIGHT_WALL;
								kingCoordinate.xCoord = myCoord.xCoord;
								kingCoordinate.yCoord = myCoord.yCoord;
							}
							sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;

						} else {
							TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
							MovementMethod.turnLeft(mRobot);
							direction = MovementMethod.changeDirectionTurnLeft(direction);
							sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;

						}
					}
					
					movementType = EXPLORE_LEFT;
				}
				
			} else if ( movementType == EXPLORE_RIGHT ) {
				
				if (  UltrasonicSensorMethod.getRightDistance(mUltrasonicSensor) < 20 ) {
					movementType = FOLLOW_LEFT_WALL;
					sendCoordinates(bOutStr, bInStr, EXPLORE_RIGHT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], 1 ,myCoord.xCoord, myCoord.yCoord) ;

				} else {
					
					int direction = 1;
					direction = MovementMethod.changeDirectionTurnLeft(direction);
					MovementMethod.turnLeft(mRobot);
					
					while ( UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor ) > 20) {
						MovementMethod.moveOneCellForward(mRobot);
						MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
						myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
						if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
							foundTheKing = EXPLORE_RIGHT;
							kingCoordinate.xCoord = myCoord.xCoord;
							kingCoordinate.yCoord = myCoord.yCoord;
						}
						sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
					}
					
					TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
					sendCoordinates(bOutStr, bInStr, EXPLORE_RIGHT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				
					int prevXcoordinate = myCoord.xCoord;
					
					while((UltrasonicSensorMethod.getBackwardDistance(mUltrasonicSensor)) > 20 && (myCoord.xCoord < 3)) {
						
						MovementMethod.moveOneCellBackward(mRobot);
						MovementMethod.updateCoordinatesWithDirectionBackwards(myCoord, direction);
						myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
						if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
							foundTheKing = EXPLORE_RIGHT;
							kingCoordinate.xCoord = myCoord.xCoord;
							kingCoordinate.yCoord = myCoord.yCoord;
						}
						sendCoordinates(bOutStr, bInStr, EXPLORE_RIGHT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord);
					}
					
					if ( myCoord.xCoord != 3 ) {
						TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
						MovementMethod.turnRight(mRobot);
						
						myCoord.xCoord = prevXcoordinate;
						sendCoordinates(bOutStr, bInStr, EXPLORE_RIGHT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;

						movementType = FOLLOW_LEFT_WALL;
					} else {
						MovementMethod.turnAround(mRobot);
						direction = MovementMethod.changeDirectionTurnAround(direction);
						TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
						MovementMethod.turnLeft(mRobot);
						movementType = FOLLOW_RIGHT_WALL;
						sendCoordinates(bOutStr, bInStr, EXPLORE_RIGHT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
					}
					


				}
				
			} else if ( movementType == EXPLORE_LEFT ) {

				if (  UltrasonicSensorMethod.getLeftDistance(mUltrasonicSensor) < 20 ) {
					movementType = FOLLOW_RIGHT_WALL;
					sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], 1 ,myCoord.xCoord, myCoord.yCoord) ;

				} else {
					
					int direction = 1;
					direction = MovementMethod.changeDirectionTurnRight(direction);
					MovementMethod.turnRight(mRobot);
					
					while ( UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor ) > 20) {
						MovementMethod.moveOneCellForward(mRobot);
						MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
						myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
						if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
							foundTheKing = EXPLORE_LEFT;
							kingCoordinate.xCoord = myCoord.xCoord;
							kingCoordinate.yCoord = myCoord.yCoord;
						}
						sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;

					}
					
					TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
					sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
					
					int prevXcoordinate = myCoord.xCoord;
					
					while(UltrasonicSensorMethod.getBackwardDistance(mUltrasonicSensor) > 20 && (myCoord.xCoord > 0)) {
						
						MovementMethod.moveOneCellBackward(mRobot);
						MovementMethod.updateCoordinatesWithDirectionBackwards(myCoord, direction);
						myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
						if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
							foundTheKing = EXPLORE_LEFT;
							kingCoordinate.xCoord = myCoord.xCoord;
							kingCoordinate.yCoord = myCoord.yCoord;
						}
						sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;

					}
					
					if ( myCoord.xCoord != 0 ) {
						TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
						MovementMethod.turnLeft(mRobot);
						
						myCoord.xCoord = prevXcoordinate;
						sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;

						movementType = FOLLOW_RIGHT_WALL;
					} else {
						MovementMethod.turnAround(mRobot);
						direction = MovementMethod.changeDirectionTurnAround(direction);
						TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
						MovementMethod.turnRight(mRobot);
						movementType = FOLLOW_LEFT_WALL;
						sendCoordinates(bOutStr, bInStr, EXPLORE_LEFT, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
					}
				}
			}
			
		}
		
		// MOVE TO THE BEGINING OF THE RAMP AND ALLIGN
		if ( myCoord.xCoord == 3 ) { // if i am in the cell next to ramp
			MovementMethod.turnRight(mRobot);
			TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
			MovementMethod.turnLeft(mRobot);
			TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
			sendCoordinates(bOutStr, bInStr, MOVE_TO_RAMP_FIRST_FLOOR, 99, 99 ,myCoord.xCoord, myCoord.yCoord) ;
		
		} else { // if i am not in the cell next to ramp
			
			int direction = 1;
			
			while ( myCoord.xCoord != 3) { // move to the cell next to the ramp
				
				int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
				int leftWallDistance = UltrasonicSensorMethod.getLeftDistance(mUltrasonicSensor);
				
				if ( leftWallDistance > 30 ) {
					
					MovementMethod.turnLeft(mRobot);
					MovementMethod.moveOneCellForward(mRobot);
					direction = MovementMethod.changeDirectionTurnLeft(direction);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
						foundTheKing = FOLLOW_LEFT_WALL;
						kingCoordinate.xCoord = myCoord.xCoord;
						kingCoordinate.yCoord = myCoord.yCoord;
					}
					sendCoordinates(bOutStr, bInStr, MOVE_TO_RAMP_FIRST_FLOOR, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;							
				
				} else if ( forwardWallDistance > 30 ) {
					
					MovementMethod.moveOneCellForward(mRobot);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
						foundTheKing = FOLLOW_LEFT_WALL;
						kingCoordinate.xCoord = myCoord.xCoord;
						kingCoordinate.yCoord = myCoord.yCoord;
					}
					sendCoordinates(bOutStr, bInStr, MOVE_TO_RAMP_FIRST_FLOOR, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
					
				} else {
					
					TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
					MovementMethod.turnRight(mRobot);
					direction = MovementMethod.changeDirectionTurnRight(direction);
					sendCoordinates(bOutStr, bInStr, MOVE_TO_RAMP_FIRST_FLOOR, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				}	
			}
			
			if(direction == 1) {
				MovementMethod.turnRight(mRobot);
				TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
				MovementMethod.turnLeft(mRobot);
				TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
			} else if (direction == 2) {
				TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
				MovementMethod.turnLeft(mRobot);
				TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
			}
			
			sendCoordinates(bOutStr, bInStr, MOVE_TO_RAMP_FIRST_FLOOR, 99, 99 ,myCoord.xCoord, myCoord.yCoord) ;
		}
	}
	
	public static void climbUpTheRamp (DataOutputStream bOutStr, DataInputStream bInStr, DifferentialPilot mRobot, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor) throws InterruptedException, IOException {
		
		sendCoordinates(bOutStr, bInStr, CLIMB_RAMP, 11, 11 , 11, 11) ;

		Motor.B.rotateTo(180);
		
		int dist = 0;
		
		while(mUltrasonicSensor.getDistance() > 20) {
			
			mRobot.travel(-30);
			Motor.B.rotateTo(90);
			dist = mUltrasonicSensor.getDistance();
			
			if (dist < 9) {
				mRobot.rotate(3);
			} else if (dist >= 10){
				mRobot.rotate(-3);
			}
			
			sendCoordinates(bOutStr, bInStr, CLIMB_RAMP, 11, 11 , 11, 11) ;
			
			Motor.B.rotateTo(180);
		}
		Motor.B.rotateTo(0);
		
		mRobot.setAcceleration(400);
		mRobot.setTravelSpeed(20); 
		
		MovementMethod.turnRight(mRobot);
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		MovementMethod.turnRight(mRobot);
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		MovementMethod.turnRight(mRobot);
		MovementMethod.moveOneCellForward(mRobot);
	}
	
	public static void climbDownTheRamp (DataOutputStream bOutStr, DataInputStream bInStr, DifferentialPilot mRobot, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor) throws InterruptedException, IOException {
		
		sendCoordinates(bOutStr, bInStr, CLIMB_RAMP_DOWN, 22, 22, 22, 22) ;
		
		int dist = 0;
		mRobot.setAcceleration(50);
		mRobot.setTravelSpeed(10);
		
		while( UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor) > 20) {
			
			mRobot.travel(15);
			Motor.B.rotateTo(90);
			dist = mUltrasonicSensor.getDistance();
			
			if (dist < 8) {
				mRobot.rotate(-3);
			} else if (dist >= 10){
				mRobot.rotate(3);
			}
		}
		
		mRobot.setAcceleration(400);
		mRobot.setTravelSpeed(20); 
		
		sendCoordinates(bOutStr, bInStr, CLIMB_RAMP_DOWN, 22, 22, 22, 22) ;
		
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		MovementMethod.turnRight(mRobot);
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		MovementMethod.turnLeft(mRobot);
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		MovementMethod.turnLeft(mRobot);
		MovementMethod.moveOneCellForward(mRobot);
	}
	
	public static void exploreSecondFloor ( Coordinates myCoord, DataOutputStream bOutStr, DataInputStream bInStr, DifferentialPilot mRobot, LightSensor mLightSensor, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor ) throws InterruptedException, IOException {
		
		myCoord.xCoord = 3;
		myCoord.yCoord = 0;
		
		Coordinates doors[] = new Coordinates[2];
		for(int i = 0; i < 2; i++ ){
			doors[i] = new Coordinates();
		}
		
		int iDoor = 0;
		int direction = 4;
		
		while(myCoord.xCoord > 0) {
			if(UltrasonicSensorMethod.getRightDistance(mUltrasonicSensor) > 20) {
				doors[iDoor].xCoord = myCoord.xCoord;
				doors[iDoor].yCoord = myCoord.yCoord;
				sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, -10, direction ,myCoord.xCoord, myCoord.yCoord) ;	
				iDoor ++;
			} else {
				sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, 0, direction ,myCoord.xCoord, myCoord.yCoord) ;
			}
			MovementMethod.moveOneCellForward(mRobot);
			MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				
		}
		
		// Down Left Cell x = 0 , y = 0
		if(UltrasonicSensorMethod.getRightDistance(mUltrasonicSensor) > 20) {
			doors[iDoor].xCoord = myCoord.xCoord;
			doors[iDoor].yCoord = myCoord.yCoord;
			iDoor ++;
			sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, -10, direction ,myCoord.xCoord, myCoord.yCoord) ;
		} else {
			sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, 0, direction ,myCoord.xCoord, myCoord.yCoord) ;
		}
		
		// I am at 0,0
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		int roomIndex = findRoomToEnter ();
		int myDoorX = doors[roomIndex].xCoord;
		
		while(myCoord.xCoord != myDoorX) {
			MovementMethod.moveOneCellBackward(mRobot);
			MovementMethod.updateCoordinatesWithDirectionBackwards(myCoord, direction);
		}
		
		MovementMethod.turnRight(mRobot);
		direction = MovementMethod.changeDirectionTurnRight(direction);
		
		MovementMethod.moveOneCellForward(mRobot);
		MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
		
		myLevel2.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel2(mLightSensor);
		if(myLevel2.Colors[myCoord.xCoord][myCoord.yCoord] == 4) {
			foundThePrincess = true;
		}
		sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, myLevel2.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
		
		while(myCoord.yCoord != 0) {
			int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
			int rightWallDistance = UltrasonicSensorMethod.getRightDistance(mUltrasonicSensor);
			
			if ( rightWallDistance > 30 ) {
				MovementMethod.turnRight(mRobot);
				MovementMethod.moveOneCellForward(mRobot);
				direction = MovementMethod.changeDirectionTurnRight(direction);
				MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				myLevel2.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel2(mLightSensor);
				if(myLevel2.Colors[myCoord.xCoord][myCoord.yCoord] == 4) {
					foundThePrincess = true;
				}
				sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, myLevel2.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;	
			} else if ( forwardWallDistance > 30 ) {
				MovementMethod.moveOneCellForward(mRobot);
				MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				myLevel2.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel2(mLightSensor);
				if(myLevel2.Colors[myCoord.xCoord][myCoord.yCoord] == 4) {
					foundThePrincess = true;
				}
				sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, myLevel2.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
			} else {
				TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
				MovementMethod.turnLeft(mRobot);
				direction = MovementMethod.changeDirectionTurnLeft(direction);
				sendCoordinates(bOutStr, bInStr, FIND_PRINCESS, myLevel2.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord);
			}
		}
		
		MovementMethod.turnLeft(mRobot);
		MovementMethod.changeDirectionTurnLeft(direction);
		
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		
		MovementMethod.turnLeft(mRobot);
		MovementMethod.changeDirectionTurnLeft(direction);
	}
	
	public static int findRoomToEnter ( ) {
		
		int firstRoom = 0;
		int secondRoom = 0;
		
		for(int i = 0 ; i < 4 ; i ++ ) {
			for(int j = 0; j < 11; j ++) {
				if ( myLevel1.Colors[i][j] == 1) {
					firstRoom++;
				} else if (myLevel1.Colors[i][j] == 2) {
					secondRoom++;
				}
			}
		}
		
		if(firstRoom >= secondRoom) {
			return 0;
		} else {
			return 1;
		}
	}
	
	public static void moveToStartingZone ( Coordinates myCoord, DataOutputStream bOutStr, DataInputStream bInStr, DifferentialPilot mRobot, LightSensor mLightSensor, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor ) throws Exception {
		
		myCoord.xCoord = 3;
		myCoord.yCoord = 10;
		
		int direction = 4;
		
		while (myCoord.yCoord != 0) {
			
			int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
			int leftWallDistance = UltrasonicSensorMethod.getLeftDistance(mUltrasonicSensor);
			
			if ( leftWallDistance > 30 ) {
				MovementMethod.turnLeft(mRobot);
				MovementMethod.moveOneCellForward(mRobot);
				direction = MovementMethod.changeDirectionTurnLeft(direction);
				MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
				if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
					myCoord.yCoord = 0;
				}
				sendCoordinates(bOutStr, bInStr, RETURN_PRINCESS_START_AREA, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;							
			} else if ( forwardWallDistance > 30 ) {
				MovementMethod.moveOneCellForward(mRobot);
				MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
				if(myLevel1.Colors[myCoord.xCoord][myCoord.yCoord] == 3) {
					myCoord.yCoord = 0;
				}
				sendCoordinates(bOutStr, bInStr, RETURN_PRINCESS_START_AREA, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
			} else {
				TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
				MovementMethod.turnRight(mRobot);
				direction = MovementMethod.changeDirectionTurnRight(direction);
				sendCoordinates(bOutStr, bInStr, RETURN_PRINCESS_START_AREA, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
			}
		}
		
	}
	
	public static void moveToTheKing ( Coordinates myCoord, DataOutputStream bOutStr, DataInputStream bInStr, DifferentialPilot mRobot, LightSensor mLightSensor, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor ) throws Exception {
		
		int direction = 4;
		
		myCoord.xCoord = 3;
		myCoord.yCoord = 10;
		
		int xKing = kingCoordinate.xCoord;
		int yKing = kingCoordinate.yCoord;
		
		if(foundTheKing == FOLLOW_LEFT_WALL || foundTheKing == EXPLORE_RIGHT) {
			
			while (myCoord.yCoord != yKing) {
				
				int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
				int rightWallDistance = UltrasonicSensorMethod.getRightDistance(mUltrasonicSensor);
				
				if ( rightWallDistance > 30 ) {
					MovementMethod.turnRight(mRobot);
					MovementMethod.moveOneCellForward(mRobot);
					direction = MovementMethod.changeDirectionTurnRight(direction);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				} else if ( forwardWallDistance > 30 ) {
					MovementMethod.moveOneCellForward(mRobot);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				} else {
					TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
					MovementMethod.turnLeft(mRobot);
					direction = MovementMethod.changeDirectionTurnLeft(direction);
					sendCoordinates(bOutStr, bInStr, FOLLOW_RIGHT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				}
			}
			
			if (foundTheKing == EXPLORE_RIGHT) {
				MovementMethod.turnLeft(mRobot);
				direction = MovementMethod.changeDirectionTurnLeft(direction);
		
			
				while(myCoord.xCoord != xKing) {
					MovementMethod.moveOneCellForward(mRobot);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				}
			}
			
		} else if (foundTheKing == FOLLOW_RIGHT_WALL || foundTheKing == EXPLORE_LEFT) {
			
			while (myCoord.yCoord != yKing) {
				
				int forwardWallDistance = UltrasonicSensorMethod.getForwardDistance(mUltrasonicSensor);
				int leftWallDistance = UltrasonicSensorMethod.getLeftDistance(mUltrasonicSensor);
				
				if ( leftWallDistance > 30 ) {
					MovementMethod.turnLeft(mRobot);
					MovementMethod.moveOneCellForward(mRobot);
					direction = MovementMethod.changeDirectionTurnLeft(direction);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;							
				} else if ( forwardWallDistance > 30 ) {
					MovementMethod.moveOneCellForward(mRobot);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
					myLevel1.Colors[myCoord.xCoord][myCoord.yCoord]=LightSensorMethod.readCellLevel1(mLightSensor);
					sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				} else {
					TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
					MovementMethod.turnRight(mRobot);
					direction = MovementMethod.changeDirectionTurnRight(direction);
					sendCoordinates(bOutStr, bInStr, FOLLOW_LEFT_WALL, myLevel1.Colors[myCoord.xCoord][myCoord.yCoord], direction ,myCoord.xCoord, myCoord.yCoord) ;
				}
			}
			
			if (foundTheKing == EXPLORE_LEFT) {
				MovementMethod.turnRight(mRobot);
				direction = MovementMethod.changeDirectionTurnRight(direction);
		
			
				while(myCoord.xCoord != xKing) {
					MovementMethod.moveOneCellForward(mRobot);
					MovementMethod.updateCoordinatesWithDirectionForwards(myCoord, direction);
				}
			}
		}
	}
	
	public static void sendCoordinates ( DataOutputStream bOutStr, DataInputStream bInStr, int movementType, int readColor , int direction , int x, int y) throws IOException {
		
		bOutStr.writeInt(movementType);
		bOutStr.flush();
		
		bOutStr.writeInt(readColor);
		bOutStr.flush();
		
		bOutStr.writeInt(direction);
		bOutStr.flush();

		bOutStr.writeInt(x);
		bOutStr.flush();

		bOutStr.writeInt(y);
		bOutStr.flush();
	}
}
